#ifndef UKF_H
#define UKF_H

#include "measurement_package.h"
#include "Eigen/Dense"
#include <vector>
#include <string>
#include <fstream>

using Eigen::MatrixXd;
using Eigen::VectorXd;

class UKF {
public:

  // initially set to false, set to true in first call of ProcessMeasurement
  bool is_initialized_;

  // if this is false, laser measurements will be ignored (except for init)
  bool use_laser_;

  // if this is false, radar measurements will be ignored (except for init)
  bool use_radar_;

  // state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
  VectorXd x_;

  // state covariance matrix
  MatrixXd P_;

  // predicted sigma points matrix
  MatrixXd Xsig_pred_;

  // time when the state is true, in us
  long long time_us_;

  // Process noise standard deviation longitudinal acceleration in m/s^2
  double std_a_;

  // Process noise standard deviation yaw acceleration in rad/s^2
  double std_yawdd_;

  // Laser measurement noise standard deviation position1 in m
  double std_laspx_;

  // Laser measurement noise standard deviation position2 in m
  double std_laspy_;

  // Radar measurement noise standard deviation radius in m
  double std_radr_;

  // Radar measurement noise standard deviation angle in rad
  double std_radphi_;

  // Radar measurement noise standard deviation radius change in m/s
  double std_radrd_ ;

  // Weights of sigma points
  VectorXd weights_;

  // State dimension
  int n_x_;

  // Augmented state dimension
  int n_aug_;

  // Sigma point spreading parameter
  double lambda_;

  // variable for 2*n_aug + 1
  int n_sigma_points_;

  // augmented sigma points matrix
  MatrixXd Xsig_aug_;

  double NIS_radar_;
  double NIS_lidar_;

  /**
   * Constructor
   */
  UKF();

  /**
   * Destructor
   */
  virtual ~UKF();

  /**
   * ProcessMeasurement
   * @param meas_package The latest measurement data of either radar or laser
   */
  void ProcessMeasurement(MeasurementPackage meas_package);

  /**
   * Prediction Predicts sigma points, the state, and the state covariance
   * matrix
   * @param delta_t Time between k and k+1 in s
   */
  void Prediction( const double delta_t );

  /**
   * Updates the state and the state covariance matrix using a laser measurement
   * @param meas_package The measurement at k+1
   */
  void UpdateLidar(MeasurementPackage meas_package);

  /**
   * Updates the state and the state covariance matrix using a radar measurement
   * @param meas_package The measurement at k+1
   */
  void UpdateRadar(MeasurementPackage meas_package);

  /**
   * Helper function to Normalize the angle 
   * @param reference to double angle 
   */
  double NormaliseAngle( double& angle );

  /**
   * High level function called to inialize state vector, can call Lidar or Radar 
   * @param meas_package
   */
  void InitializeStateVector( MeasurementPackage meas_package );
  
  /**
   * Helper function to convert from Polar to Cartesion  
   * @param const reference to VectorXd v_in 
   */
  VectorXd PolarToCartesian( const VectorXd& v_in );

  /**
   * Initialize Radar Vector  
   * @param const reference to VectorXd v_in 
   */
  VectorXd InitializeRadarVector( const VectorXd& v_in );

  /**
   * Initialize Lidar Vector  
   * @param const reference to VectorXd v_in 
   */
  VectorXd InitializeLidarVector( const VectorXd& v_in );

  /**
   * Create Augmented Sigma points  
   * @param none 
   */
  void CreateAugmentedSigmaPoints();

  /**
   * Presdict Sigma points  
   * @param double delta_t 
   */
  void PredictSigmaPoints( const double delta_t );

  /**
   * Predict mean and covariance   
   * @param none 
   */
  void PredictMeanAndCovariance();
};

#endif /* UKF_H */
